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Abstract 

Mixed integer linear programming (MILP) is a powerful tool for plan- 
ning and control problems because of its modeling capability and the avail- 
ability of good solvers. However, for large models, MILP methods suffer 
computationally. In this paper, we present iterative MILP algorithms 
that address this issue. We consider trajectory generation problems with 
obstacle avoidance requirements and minimum time trajectory generation 
problems. The algorithms use fewer binary variables than standard MILP 
methods and require less computational effort. 



1 Introduction 

Mixed integer linear programming (MILP) methods have attracted attention 
because of their modeling capability and because powerful solvers are available 
commercially. The utilization of MILP for modeling and control problems is 
described in and for hybrid systems and practical applications in 17 . MILP 
methods are used in |19) for cooperative reconnaissance, in |2(J| for spacecraft 
path planning, and in 0^11^2 ^ or C00 P era tive control problems. 

Powerful software packages such as CPLEX solve MILPs efficiently for 
problems in which the number of binary variables is of reasonable size. How- 
ever, a major disadvantage of MILP is its computational complexity Because 
MILP is NP-hard in the number of binary variables used in the problem for- 
mulation |14j . computational requirements grow significantly as the number of 
binary variables needed to model the problem increases. Motivated to gener- 
ate efficient MILP problem formulations, we have developed several iterative 
techniques that require fewer binary variables than standard MILP methods. 

The MILP obstacle avoidance methods from [201 an d those from |X(JI 
developed independently, specify a uniformly distributed set of discrete times 
at which obstacle avoidance is enforced. We call this approach uniform grid- 
ding. In this approach, there is no avoidance guarantee between time steps. 
In addition, many of the avoidance times are unnecessary, resulting in large 
MILPs that require a significant computational effort to solve. Here, we present 
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an iterative MILP obstacle avoidance algorithm that can be used alone or in 
combination with the uniform gridding approach. The algorithm guarantees 
obstacle avoidance over the entire trajectory and distributes avoidance times ef- 
ficiently, resulting in smaller MILPs that can be solved faster. We also present 
an iterative MILP obstacle growing algorithm that allows the use of a coarse set 
of uniformly distributed obstacle avoidance times. In this approach, collision 
free trajectories are found by artificially increasing the size of the obstacles that 
collide with the trajectory generated by the MILP, iterating until the resulting 
trajectory is collision free. 

Next, we consider the minimum time trajectory generation problem using 
MILP. The MILP approach to this problem presented in |21l 122) generates an 
approximate solution. Time is discretized uniformly, and an auxiliary binary 
variable and a set of inequality constraints are added for each discrete time. This 
approach gives an estimate to the time optimal solution that depends on the 
sampling time chosen. For more accuracy, the sampling time is reduced, which 
results in a larger number of binary variables in the MILP formulation and 
thus increases the computation time, possibly exponentially. Here, we present 
an iterative MILP algorithm that solves for the time optimal solution to the 
problem. The algorithm uses binary search. At each iteration, the feasibility of 
a MILP with only one discrete time (for the minimum time part of the problem) 
needs to be determined. 

The paper is organized as follows: In Section |2J we describe the dynamics 
of the vehicles we use to motivate our methods. In Section |3| we describe two 
iterative MILP algorithms for obstacle avoidance, and we perform an average 
case computational complexity study comparing the performance of the iterative 
time step selection algorithm with the uniform gridding approach. Finally, in 
Section 21 we describe an iterative MILP algorithm for minimum time control 
problems. All files for generating the plots found in this paper are available 
online |12| . 

2 Vehicle dynamics 

We motivate our methods using the wheeled robots of Cornell's RoboCup Team 
12,'ij . In this section, we show how to simplify their nonlinear governing equations 
using a procedure from |18| . The result is a linear set of governing equations 
coupled by a nonlinear constraint on the control input. This procedure allows 
real-time calculation of many near-optimal trajectories and is a major factor 
for Cornell's success in the RoboCup competition. We then show how to repre- 
sent the simplified system in a MILP problem formulation. The result is a set 
of linear discrete time governing equations subject to a set of linear inequality 
constraints. 

Each vehicle is equipped with a three-motor omni-directional drive, which 
allows it to move along any direction irrespective of its orientation. This al- 
lows superior maneuverability compared to traditional nonholonomic (car-like) 
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vehicles. The non-dimensional governing equations of each vehicle are given by 
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and U(t) = (Ui(t),U2(t),Us(t)) G In these equations (x(t),y(tj) are the 
coordinates of the vehicle, 0(f) is its orientation, u(9(t), t) is the 0(i)-dependent 
control input, m is the mass of the vehicle, / is its moment of inertia, L is the 
distance from the drive to the center of mass, and Ui(t) is the voltage applied 
to motor i. The set of admissible voltages U is the unit cube, and the set of 
admissible control inputs is given by P{9)U. 

These governing equations are coupled and nonlinear. To simplify them, 
we replace the set P(9)U with the maximal ^-independent set found by taking 
the intersection of all possible sets of admissible controls. The result is a 0- 
independent control set defined by control input (u x (t),u y (t),U0(t)) and the 
inequality constraints u x (t) 2 + u y (t) 2 < (3 — \ug(t)\) 2 /4 and < 3. Using 

the restricted set as the allowable control set, the governing equations decouple 
and are given by 
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The constraints on the control input couple the degrees of freedom. 

To decouple the 9 dynamics we further restrict the admissible control set to 
a cylinder defined by the following two inequalities: |ue(t)| < 1 and 



U X (t) 2 +Uy{t) 2 < 1. 



(3) 



Now, the equations of motion for the translational dynamics of the vehicle are 
given by 



x(t) + x(t) = u x {t), 
y(t) + y(t) = u y (t), 



(4) 



subject to equation J2J). In state space form, equation Q is x(t) = A c x(i) + 
B c ii(f), where x = (x, y, x, y) is the state and u = (u x , u y ) is the control input. 

To represent the governing equations in a MILP framework, we discretize 
the control input in time. We require the control input be constant between 
time steps. The result is a set of linear discrete time governing equations, which 
we derive next. 
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Let N u be the number of discretization steps for the control input u(i). Let 
t u [k] be the time at step k. Let T u [k] > be the time between steps k and fc+1, 
for k G {0, . . . , N u — 1}. The discrete time governing equations are given by 

x u [fc + 1] = A[fc]x„[fe] + B[k]u[k], (5) 

where x„[fe] = x(i u [fc]), u[fc] = u(t u [/c]), x„[fc] = (x„[fc], y u [fc], i„[fc], y u [k]), and 
u[fc] = (ua;[fc], Uy[k\). The coefficients A[fc] and B[fc] are functions of k because 
we have allowed for nonuniform time discretizations. They can be calculated 
explicitly in the usual way Because there will be several different time 
discretizations used in this paper, we use subscripts to differentiate them. In 
this section, we use the subscript u to denote variables associated with the 
discretization in the control input u(t). 

The discrete time governing equations can be solved explicitly in the usual 
way [B]- In later sections of this paper, it will be necessary to represent the 
position of the vehicle, at times between control discretization steps, in terms 
of the control input. Because the set of governing equations is linear, given the 
discrete state x„[fc] and the control input u[fe], we can calculate the vehicle's 
state at any time t using the following equations: 

x(t)=x u {k] + (l-e t ^- t )x u [k] 

+ (t- t u [k] - 1 + e *«[*]-*) Ua .[fc] s 
x(t) = {e^-*)x u [k] + (1 - e^W^Kffc], (6) 

where k satisfies t u [k] < t < t u [k + 1], If the time discretization of the control 
input is uniform, T u [k u ] = T u for all k u , then k u — \ t/T u \. The components of 
the vehicle's state, y(t) and y(t), can be calculated in a similar way. 

The control input constraint given by equation @ cannot be expressed in 
a MILP framework because it is nonlinear. To incorporate this constraint, we 
approximate it with a set of linear inequalities that define a polygon. The 
polygon inscribes the region defined by the nonlinear constraint. We take the 
conservative inscribing polygon to guarantee that the set of allowable controls 
defined by the region is feasible. Similar to work in [21| . we define the polygon 
by the set of M u linear inequality constraints 

u x \k \ sin — — hu» « cos —r-r- < cos -rr- 

Vme {1,...,M U }, (7) 

for each step fee {1, . . . , N u }. 

To illustrate the approach, consider the following minimum control effort tra- 
jectory generation problem. Given a vehicle governed by equations © and Q, 
find the sequence of control inputs {uffc]}^^ 1 that transfers the vehicle from 
starting state x(0) = x s to finishing state x(t/) = X/ and minimizes the cost 
function 

N u -1 

j= (KWI + KWI). (8) 

fc=0 
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To convert the absolute values in the cost function to linear form, we in- 
troduce auxiliary continuous variables z x [k] and z y [k] and the inequality con- 
straints 

-z x [k] < u x [k] < z x [k] 

—z y [k] < u y [k] < z y [k]. (9) 

Minimizing z x [k] subject to the inequalities u x [k] < z x [k] and u x [k] > — z x [k] is 
equivalent to minimizing |ua;[fc]| (similarly for |ti y [fc]|) Using the auxiliary 
variables, the cost function can be written as a linear function, 

N u -1 

J= (z x [k] + z y [k}) . (10) 

k=0 

The resulting optimization problem (minimize I|1U|) subject to J3J), J7J), ©, 

and the boundary conditions) is in MILP form. Because binary variables do not 
appear in the problem formulation, it is a linear program and is easily solved to 
obtain the optimal sequence of control inputs. 

3 Obstacle avoidance 

In vehicle control, it is necessary to avoid other vehicles, stationary and moving 
obstacles, and restricted regions. In this section, we show how to use MILP 
to solve obstacle avoidance problems, we present two iterative MILP obsta- 
cle avoidance algorithms that are more computationally efficient than standard 
methods, and we perform an average case computational complexity study. 

3.1 MILP formulation 

We start by showing a MILP method to guarantee circular obstacle avoidance 
at N a discrete times. A version of this method for uniformly distributed ob- 
stacle avoidance times is presented in |2U) . and a similar method is presented 
independently in |1(JI II lj . The method we present here allows nonuniform dis- 
tributions of obstacle avoidance times 0, which we take advantage of in our 
iterative algorithm presented in the next section. We use subscript o to denote 
variables associated with the time discretization for obstacle avoidance. For 
step k, taken to be an clement of the set {1, . . . , N }, let t [k] be the time at 
which obstacle avoidance is enforced. Let Robst denote the radius of the obsta- 
cle, and let (x b s t[k],y bst[k]) denote the coordinates of its center at time t [k\. 
We approximate the obstacle with a polygon, denoted 0[k], defined by a set of 
M Q inequalities. The polygon is given by 

0[k]={ (x,y) : {x-x obst [k])sm^ 

+ (y-Vobstik}) COS < Rabat", (11) 

Vme {!,..., M } }. ° 
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To guarantee obstacle avoidance at time t [k] the coordinates of the vehicle 
must be outside the region 0[k]. This avoidance condition can be written as 
(x [k},y [k]) £ 0[k], where (x [k] , y [k] ) are the coordinates of the vehicle at 
time t [k). Here x [k) = x(t [k]) and y [k] = y(t [k)) are expressed in terms of 
the control inputs using equation ijfJJl. 

Because at least one constraint defining the region 0[k] must be violated 
in order to avoid the obstacle, the avoidance condition is equivalent to the 
following condition: there exists an m such that (x D [fc] — x obst [k]) sin + 

(y [k] - y bst[k}) cos ^g* > R obst . 

To express this avoidance constraint in a MILP problem formulation, it 
must be converted to an equivalent set of linear inequality constraints. We do 
so by introducing auxiliary binary variable b m [k] S {0, 1} and the following M Q 
inequality constraints: 

[x [k] - x obst [k]) sin + (y [k] - y obst [k])cos ^ 

> R obst - Hb m [k], Vme {1,...,M }, 1 > 

where H is a large positive number taken to be larger than the maximum di- 
mension of the vehicle's operating environment plus the radius of the obstacle. 
If b m [k] = 1, the right side of the inequality is a large, negative number that 
is always less than the left side. In this case, the inequality is inactive because 
it is trivially satisfied. If b m [k] = 0, the inequality is said to be active because 
it reduces to an inequality from the existence condition above. For obstacle 
avoidance, at least one of the constraints in equation (|12() must be active. To 
enforce this, we introduce the following inequality constraint into the problem 
formulation: 

M a 

J2 b m[k]<M -l. (13) 

Therefore, to enforce obstacle avoidance at time t [k], the set of binary 
variables {b m [k\\^=i an< ^ the constraints given by equations l|12f> and l|13fl are 
added to the MILP problem formulation. 

Consider the example problem from Section [21 adding obstacles that must 
be avoided. In this problem, we want to transfer the vehicle from start state x s 
to finish state x^- in time tf using minimal control effort and avoiding obstacles. 
To enforce obstacle avoidance at each time in the set {£o[fc]}£i, we augment 
the MILP formulation in Sectional with the set of binary variables {&m[&]}™=i, 
constraints (|12|l . and constraint (|13fl for all k in the set {1, . . . , N a }. 

Distributing the avoidance times uniformly (uniform gridding) results in a 
trajectory that avoids obstacles at each discrete time in the set. However, the 
trajectory can collide with obstacles between avoidance times. This is shown 
for an example instance in Figure ^a). 

A simple method to reduce this behavior is to take a finer discretization, 
which increases the number avoidance times, as shown in Figure^b). However, 
this is not desirable in MILP because an increase in the number of avoidance 
times increases the number of binary variables in the problem. 
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Figure 1: Figure (a) shows the resulting trajectory using uniform gridding (N a = 
10), Figure (b) shows the trajectory using a finer uniform gridding (N a = 50), 
and Figure (c) shows the trajectory using iterative MILP avoidance (N a = 9). 
The circles denote obstacles, and the polygons denote the buffer regions used in 
the MILP formulation. The values of the parameters are M a = 6, M u = 4, N u = 
4, {x s ,y s ,x s ,y s ) = (-0.25, -0.2, -0.5, 0.3), and {x f ,y f ,Xf,y f ) = (0.4,0.3,0,0). 
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Table 1: Iterative MILP time step selection algorithm 



1: Formulate vehicle control problem as a MILP 
with the set of obstacle avoidance times 

{to[k]}k=V 

2: Set obstacle buffer zone for each obstacle j, 
R bu ff -a^alt where a >1. 

3: Solve MILP with obstacles of radius R^tf f° r 
each obstacle j. 

4: Check resulting trajectory for collisions with ob- 
stacles of radius R$ st for each obstacle j. 

5: while there are collisions do 

6: For each collision i, compute time interval 

7: For each collision i, augment the MILP formu- 
lation with obstacle avoidance constraints at 
time t n Q ew G ,tf\. 

8: Solve augmented MILP with obstacles of radius 

(i) 

Rfruft for each obstacle j. 
9: Check resulting trajectory for collisions with 

obstacles of radius R^ st for each obstacle j. 
10: end while 



3.2 Iterative MILP time step selection algorithm 

It is advantageous to use as few avoidance times as possible. Next, we pro- 
pose an iterative algorithm to do so. The method distributes avoidance times 
where they are needed most, as shown in Figure ^c), and guarantees obsta- 
cle avoidance if an obstacle free trajectory exists. The idea is to first solve the 
MILP with no obstacle avoidance times (or with a coarse set of avoidance times) 
and check the resulting trajectory for collisions. Then, if there are collisions, 
augment the MILP formulation with an avoidance time (and the correspond- 
ing binary variables and constraints) for each collision. The avoidance time for 
each collision is taken from the interval of time that the trajectory is within the 
obstacle. Next, solve the augmented MILP and check the resulting trajectory 
for collisions, repeating the procedure until a collision free trajectory is found. 

The algorithm is outlined in Tableland proceeds as follows: First, formulate 
the vehicle control problem as a MILP and choose an initial set of avoidance 
times {t [fe]}i.=i- This set is usually taken to be the empty set or a coarsely 
distributed set of times. Next, introduce a buffer zone for each obstacle j with 
radius R^ff = a R < obsV wnere a > 1 is the buffer factor. Radius R^ff ^ s l ar g er 
than R^ s t (usually taken slightly larger) and is used as the radius of obstacle 
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Figure 2: Snapshots of the iterative MILP obstacle avoidance algorithm. The 
circular regions denote obstacles, and the polygons denote the buffer regions 
used in the MILP formulation. Each cross ' x ' denotes a time at which obstacle 
avoidance is enforced. The values of the parameters are M a = 6, M u = 4, N u = 
4, (x s ,y s ,x s ,y s ) = (-0.25, -0.2, -0.5, 0.3), and (x f ,y f ,x f ,y f ) = (0.4,0.3,0,0). 



j in the MILP formulation. This is done to guarantee obstacle avoidance and 
termination of the algorithm, which we show later in this section. Next, solve 

the MILP using the buffer regions as the obstacles. Then, check the resulting 

(i) 

trajectory for collisions using each obstacle's true radius, RJ bst for each obstacle 
j. To check for collisions, sample the trajectory and check whether or not each 
sample point is inside any of the obstacles. 

If there are no collisions, terminate the algorithm. Otherwise, for each col- 
lision i, compute the time interval [i^,^] m which the trajectory is within 
the obstacle. This interval can be computed efficiently using a bisection routine 
and the collision check routine. Then, for each collision i, augment the MILP 
problem formulation with avoidance constraints at time t™ ew taken to be in the 
interval In this paper, we take t™ ew = {tf +t ( * ] )/2. Next, solve the 

augmented MILP and check the resulting trajectory for collisions. If there are 
no collisions, terminate the algorithm. Otherwise, repeat the procedure until 
there are no collisions. 

Snapshots of intermediate steps in the iterative algorithm are shown in Fig- 
ure [21 The procedure adds obstacle avoidance points where they are needed 
most, thus avoiding unnecessary and computationally costly constraints and 
binary variables. 
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t Q [k + 1] - t a [k] > 2Ai min t a [k + 1] - t a [k] < 2At min 

Figure 3: These diagrams help show that the iterative MILP time selection 
algorithm terminates. If the difference between consecutive obstacle avoidance 
times, denoted t [k+ 1] — t a [k], is greater than 2Ai m ; n , the trajectory can inter- 
sect the obstacle as shown in the figure on the left. In this case, the algorithm 
will add a new avoidance constraint at time t™ ew . If the difference is less than 
2At m ; n , the trajectory can not intersect the obstacle, and a new avoidance con- 
straint can not be added in between. 

Now we show that the iterative algorithm in Table ^ terminates. The 
minimum distance between the boundary of a buffer zone and the boundary 
of the obstacle it surrounds is d — Rbuff — Robst = (a — l)R b s t. For a 
problem involving multiple obstacles, the minimum of these distances is given 
by dmin = (a — l)-Ro"'" t , where R™f t is the radius of the smallest obstacle 
in the environment. The minimum time it takes the vehicle to travel be- 
tween the boundary of a buffer zone and its corresponding obstacle is given 
by Ai min = d mhl /v max = (a - l)R^ t /v max , where v maK is the maximum veloc- 
ity of the vehicle. Consider two consecutive obstacle avoidance times denoted 
t Q [k] and t [k + 1]. The vehicle must be located outside all buffer zones at these 
two times because we have enforced this as a hard constraint in the MILP. If 
the difference t [k + 1] — t Q [k] is less than 2Ai m ; n , the vehicle's trajectory can 
not intersect the obstacle because there is not enough time to enter the buffer 
zone, collide with the obstacle, then exit the buffer zone (see Figure 0). In 
order for the trajectory to intersect the obstacle in the interval between these 
two times, the difference t [k + 1] — t Q [k] must be greater than 2Ai m ; n . In 
summary, the algorithm will not add an obstacle avoidance time in the inter- 
val if t [k + 1] — t [k] < 2Ai m i n , but it can add an obstacle avoidance time if 
t [k + 1] — t [k] > 2Ai m ; n . Therefore, in the worst case, once the algorithm 
reaches a point where the time interval between each obstacle avoidance time is 
less than 2At m j n , the algorithm must terminate. 

Next we bound the number of steps it takes for the algorithm to terminate. 
The smallest possible time interval between consecutive obstacle avoidance times 
is Aimi n . This can be seen by looking at Figure 31 where t D [k + 1] and t [k] 
are two consecutive avoidance times and t\ is the time at which the trajectory 
enters the obstacle and t<i is the time it exits the obstacle. Suppose the vehicle 
is moving at its maximum velocity from time t [k] to t\. The algorithm will 
detect this intersection, compute times t\ and t%, and pick a new obstacle avoid- 
ance time t™ ew in the interval [ti,t2]- Suppose the algorithm picks t™ ew = t%, 



10 



Figure 4: This diagram helps show the minimum time step that can be added by 
the iterative MILP time step selection algorithm. The trajectory intersects the 
obstacle in the interval [ii,^]- Assume the vehicle is moving at u max between 
times t [k] and t\. If the algorithm selects t\ as the new obstacle avoidance 
time, the difference t\ — t [k] is equal to At m j n . This is the minimum possible 
time interval between avoidance times because the vehicle can not move any 
faster. 

then t^ ew — t Q [k] = At m j n . The time interval can not be any less because the 
vehicle can not pass through the buffer zone in time less than At m ; n . In the 
trajectory generation problem, if t s is the vehicle's starting time and tf is its 
finishing time, the maximum number of time intervals added by the algorithm 
is \_(tf — - t s )/ At m i n \. Therefore, the algorithm will terminate in a maximum of 
L(i/ — t s )/Ai m i n J steps. This is a worst case result. In practice the algorithm 
terminates in fewer steps. 

3.3 Iterative MILP obstacle growing algorithm 

Being consistent with our goal to reduce the number of obstacle avoidance times 
in our MILP problem formulations, we propose another iterative MILP algo- 
rithm for obstacle avoidance. This algorithm iteratively grows the buffer zones 
surrounding the obstacles until a collision free trajectory is found. The idea is 
to first solve the MILP with a coarse set of avoidance times and an initial set 
of buffer zones surrounding each obstacle. Then, check the resulting trajectory 
for collisions. If there are collisions, increase the size of each buffer zone that 
surrounds an obstacle with which the trajectory collides. Next, solve the MILP 
with these new buffer zones and check the resulting trajectory for collisions. 
This process is repeated until there are no collisions. 

The details of the algorithm are listed in Table Snapshots of intermediate 
steps of the algorithm are shown in Figure The crosses denote the coarse 
set of times at which obstacle avoidance is enforced in the MILP. As the figure 
shows, the size of the buffer regions surrounding the obstacles with which the 
trajectory intersects is increased until the resulting trajectory, generated by the 
MILP, avoids all obstacles. 

The situation in which this algorithm is most useful is when uniform gridding 
is used and the resulting trajectory clips an obstacle, barely intersecting it. In 
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Table 2: Iterative MILP obstacle growing algorithm 



1: Formulate vehicle control problem as a MILP 
with the set of obstacle avoidance times 

{to[k]}k=V 

2: Set obstacle buffer zone for each obstacle j, 
R bu ff -a^alt where a >1. 

3: Solve MILP with obstacles of radius R^uff f° r 
each obstacle j. 

4: Check resulting trajectory for collisions with ob- 
stacles of radius R^J st for each obstacle j. 

5: while there are collisions do 

6: For each obstacle j that collides with the trajec- 
tory, increase buffer region by setting R^ff := 

aR buff 

7: Solve MILP with obstacles of radius R^ff f° r 

each obstacle j. 
8: Check resulting trajectory for collisions with 

obstacles of radius R^ st for each obstacle j. 
9: end while 



this case, the algorithm pushes the trajectory away from the clipped obstacle in 
a few iterations, resulting in a collision free trajectory. However, if the initial 
distribution of avoidance times is too coarse, the algorithm could have problems. 
In this case, the buffer regions could grow to be large and engulf the initial or 
final position, which results in an infeasible MILP. 

3.4 Average case complexity 

In this section, we explore the average case computational complexity of the 
iterative MILP obstacle avoidance algorithm by solving randomly generated 
problem instances. Each instance is generated by randomly picking parameters 
from a uniform distribution over the intervals defined below. Each MILP is 
solved using AMPL [T3] and CPLEX [T^ on a PC with Intel PHI 550MHz 
processor, 1024KB cache, 3.8GB RAM, and Red Hat Linux. For all instances 
solved, processor speed was the limiting factor, not memory. 

For comparison, we solve the same instances using uniform gridding with 
sample time At c = 2R™f t ^J a 2 — l/v maK . This sample time is the maximum 
sample time that guarantees obstacle avoidance, assuming the vehicle travels 
in a straight line between sample times. This is a good approximation since 
At c is small for the instances we solve. See Appendix for details. Each 
obstacle avoidance time is given by t [k] = kAt c , where k = 1, ...,N a and 
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Figure 5: Snapshots of the iterative MILP obstacle growing algorithm. The 
circular regions denote obstacles, and the polygons denote the buffer regions 
used in the MILP formulation. Each cross denotes a time at which obstacle 
avoidance is enforced. 

iV = \t f /At c ]. 

The instances are generated as follows: The start state is taken to be 
x s = (x s , y s , r v cos 9 Vl r v sin0„), where (x s ,y s ) is constant, and r v and 9 V are 
random variables chosen uniformly from the intervals [rjf m ,r™ ax ] and (0, 2ir], 
respectively. The final state is fixed with zero velocity, x/ = [xf, yf, 0, 0). We 
generate N b st obstacles each with position {x b s t,y bst) — (r cos 9, r sin (9) and 
radius R bst- The parameters R bst, r, and 9 are random variables chosen uni- 
formly from the respective intervals [R m in , Rmax] , [fnum fmaxL and (0,27r] such 
that no obstacle overlaps the circle of radius R s with position (x Sl y s ) or the 
circle of radius Rf with position (xf,yf). 

For the instances generated in this paper, we set the intervals to be r v S 
[0.5, 1.0], Robst S [0.2, 0.3], and r € [0.0, 1.0]. The constant parameters are taken 
to be (x s ,y s ) = (-0.8,-0.8), (x f ,y f ) = (1.0,1.0), R s = 0.5, and R f = 0.1. 

The solution to an instance of the obstacle avoidance problem with three 
obstacles is shown in Figure |H1 for the the uniform gridding method and for the 
iterative MILP methods. Each cross denotes the time along the trajectory at 
which obstacle avoidance is enforced. The uniform gridding method with sam- 
ple time At c requires N Q = 25 obstacle avoidance times, shown in Figure E^b), 
while the iterative MILP time step selection algorithm requires only N D = 4 
avoidance times, shown in Figure El^c). Notice the efficiency in which the iter- 
ative algorithm distributes the avoidance times. For comparison, we also solve 
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(a) (b) 




Figure 6: Solutions to an instance of the obstacle avoidance problem using: (a) 
uniform gridding with sample time 2Ai m j n (N a — 115), (b) uniform gridding 
with sample time At c (N Q = 25), (c) iterative time step selection (N Q = 4), and 
(d) iterative obstacle growing (N — 5). The straight line segment denotes the 
initial velocity (x s ,y s ) = (0.497,0.172), the circular regions denote obstacles, 
the dashed regions denote the polygonal buffer zones, and each cross denotes a 
time along the trajectory at which obstacle avoidance is enforced. 

this instance using uniform gridding with sample time 2Ai m j n (Figure EJa)) 
and using the iterative obstacle growing algorithm (Figure Eld)). For uniform 
gridding, choosing sample time 2At m ; n guarantees obstacle avoidance as dis- 
cussed in Section HOI However, as shown in the figure, this dense set of obstacle 
avoidance times is very conservative. 

In Figure[3 we plot the fraction of instances solved versus computation time 
for the two methods. As these figures show, the iterative MILP method is less 
computationally intensive than the uniform gridding method for the instances 
solved. For example, 70% of the instances are solved in 0.4 seconds or less using 
the iterative MILP algorithm for the 3 obstacle case. In contrast, no instances 
are solved in 0.4 seconds or less using uniform gridding for the 3 obstacle case. 

In Figure |H1 we plot the computation time necessary to solve 70% of the 
randomly generated instances versus the number of obstacles on the field. Data 
is plotted for the uniform gridding method and for the iterative MILP method. 
The computational requirements for both methods grow exponentially with the 
number of obstacles. However, as the figure shows, the iterative MILP method 
is less computationally intensive and the computation time grows at a slower 
rate. 
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Figure 7: Fraction of instances solved versus computation time for uniform 
gridding (top) and the iterative MILP obstacle avoidance (bottom) . We consider 
N bst — 2,3,4. For each curve, 500 random instances were solved. The values 
of the parameters are N u = 10, M u = 10, and M a = 10. 
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Figure 8: Computation time necessary to solve 70% of the randomly generated 
instances versus the number of obstacles. Each square denotes a data point 
for the uniform gridding method. Each asterisk denotes a data point for the 
iterative MILP method. The solid lines denote curves fitted to these data. 

4 Minimum time problems 

In this section, we present an iterative MILP algorithm for solving minimum 
time problems using a vehicle trajectory generation problem as motivation. 
In MILP methods for this problem are presented. Time is discretized 

uniformly and the sampling interval that contains the optimal time is found 
using MILP. To get better bounds on the optimal time, the sample time of 
the discretization must be reduced, which results in a larger number of binary 
variables. In Appendix El this method is outlined in the context of the vehicle 
considered in this paper. 

Here we propose an iterative algorithm that converges to the optimal time 
using binary search. At each iteration the feasibility of a MILP is determined 
using a solver such as CPLEX 15 . In each MILP, the number of binary variables 
and the number of constraints are much fewer than those for other techniques 
because only one discrete time step is needed. 

To motivate the iterative algorithm we consider a minimum time vehicle 
control problem. Given a vehicle governed by equations (J5J and Q, find the 
sequence of control inputs {uffc]}^^ 1 that transfers the vehicle from initial 
state x(0) = x s to final state x(i/) = xy in minimum time. 

Suppose we know that the optimal time, denoted t*, is within the time 
interval fa, tit]. Let time tu — fa + tn)/2. Consider the MILP given by 
equation (J5J, equation Q, constraint x(0) = x s , and constraint x(i/) = x/ 
with final time taken to be tf = tu- We use equation JBJ to express x(i/) in 
terms of the control inputs. To determine if there exists a sequence of control 
inputs that transfers the vehicle from start state to finish state, we solve the 
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Table 3: Iterative minimum time MILP algorithm 



1 


Formulate problem as a MILP without ob- 




jective function. 


2 


Set th := tw and tR :=*«&. 


3 


Set tu ■= (tR + t L )/2. 


4 


while (tu — ti) > e do 


5 


Determine feasibility of MILP with final 




time tf = tu- 


6 


if feasible then set tR := tm- 


7 


else set := tM- 


8 


Set t M :={t R + t L )/2. 


9 


end while 



MILP without an objective function (this is a feasibility problem). 

If the MILP is feasible, t* must be within the interval (tz,,iM]- Otherwise, 
the MILP is infeasible and t* must be within the interval (<m , tu] ■ By deter- 
mining the feasibility of the MILP, we have cut the bound on the optimal time 
t* in half. This suggests an iterative binary search procedure that converges to 
t*. 

The iterative algorithm is outlined in Tableland proceeds as follows: First, 
pick a time interval (tw, t u b] that bounds the optimal time t*. The lower bound 
is taken to be tw = cZ m i n /i> ma x, where d min is the straight line distance from the 
initial position to the final position and i> m ax is the maximum velocity of the 
vehicle. The upper bound is taken to be a feasible time in which the vehicle 
can reach the destination. A simple way to compute a feasible time is to try 
time atib, where a > 1, increasing a until a feasible time is found. Set tt '■= tu,, 
tR := tub, and t M ■= (tR + t L )/2. 

Next, set the final time in the MILP problem formulation to be tf = tM, 
and determine if the resulting MILP is feasible using the MILP solver. If the 
MILP is feasible, the optimal time t* must be within the interval (fz,,tA/]- I n 
this case, set tR := tM- Otherwise, the MILP is infeasible and the vehicle can 
not reach the destination in time tjvf. The optimal time t* must be within 
the interval (tM,t R ]- In this case, set t^ :— tM- Then, update tM by setting 
tM '■= (tR + tt,)/2. If the difference tR — tL is less than some desired tolerance 
for our calculation of t* , denoted e, the algorithm terminates. Otherwise, repeat 
the process by setting the final time to tf = tM and continue with the steps 
outlined previously until the computed value of t* is within the desired tolerance 
e. 

After the fcth iteration, the time interval containing optimal time t* has 
length (t u b - tw)/2 k . 

The solid lines of Figure El show the solution to an instance of the minimum 
time problem. The iterative procedure was stopped after thirteen iterations, 
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Figure 9: Time optimal solution (solid lines) to an instance of the minimum 
time vehicle control problem of Section 0] given by the iterative MILP algo- 
rithm. For comparison we plot the near optimal solution (dotted lines) for the 
continuous time version of the problem obtained using techniques from |18| . The 
parameters are: M u = 20, N u — 10, (x s , y s , x s , y s ) = (—0.25,-0.2,-0.5,0.3), 
(x f ,Vf,±f,y f ) = (0.4,0.3,0,0). 

which took approximately one second on our Pentium III 550 MHz computer. 
To achieve the same accuracy using the uniform time discretization method, 
solving one large MILP with a small sampling time, it took five minutes on the 
same computer. 

Our iterative procedure converges to the time optimal solution of the prob- 
lem stated in the beginning of this section. This solution is an approximate 
solution to the continuous time version of the minimum time vehicle control 
problem. In the continuous time version of the problem, the vehicle is governed 
by equations (@J and © . We wish to transfer the vehicle from starting state x s 
to finishing state in minimum time. In Figure |5| we compare our near opti- 
mal solution to the continuous time problem (solid lines) to another technique 
(dotted lines) for generating near optimal solutions from |18j . which was used 
successfully in the RoboCup competition. 

In addition to being used on its own, our iterative approach can be combined 
with the uniform discretization approach. In this case, the uniform approach is 
run first with a coarse discretization (large sampling time T). The output is a 
time interval of size T, which contains the optimal time t*. We use this time 
interval as the input to our iterative algorithm. The A:th step of the iterative 
algorithm outputs a time interval of length T/2 k containing the optimal time 
t*, and thus quickly converges to the optimal time. 



18 



5 Discussion 

We have presented iterative MILP algorithms for obstacle avoidance and for 
minimum time control problems. The iterative MILP time selection algorithm 
picks obstacle avoidance times and intelligently distributes them where they are 
needed most. The iterative MILP obstacle growing algorithm allows a course 
set of obstacle avoidance times to be used instead of a dense distribution, which 
is required to guarantee obstacle avoidance for standard MILP methods. Both 
of these algorithms reduce the number of binary variables needed to formu- 
late and solve obstacle avoidance trajectory generation problems using MILP. 
To demonstrate the computational benefits of the iterative MILP time step 
selection algorithm, we performed an average case computational complexity 
analysis. For comparison, we also performed the analysis on the standard uni- 
form gridding method. The iterative algorithm significantly outperformed the 
uniform gridding method. In addition, we also present an iterative algorithm 
for solving minimum time problems using MILP. We found that the algorithm 
significantly outperforms standard techniques for minimum time problems using 
MILP. 

Due to the reduced computational requirements of these methods, they can 
be applied more widely in practice. Computational efficiency is especially im- 
portant for real time control in dynamically changing environments where new 
control plans need to be generated often and in real time using a strategy such 
as model predictive control [T^. hi our research |5J|U], we use these methods 
to solve cooperative control problems such as those described in 0J|SJ[7]. How- 
ever, there is much room for improvement, including decreasing computation 
time further and developing methods that scale better with increased numbers 
of obstacles and vehicles. In |S] we discuss ideas to further decrease the com- 
putational requirements of MILP methods. We feel that intelligent time step 
selection methods, such as those presented in this paper, can be very useful 
in reducing computational requirements and should be pursued further. One 
aspect that needs inspection is the intelligent selection of the discretization for 
the control input to the vehicle. 



A Appendix: Sample time 

Here we derive the minimum sample time, denoted Ai c , that guarantees obstacle 
avoidance between sample times, assuming the vehicle moves in a straight line 
path between sample times. This is a good approximation, because Ai c is small 
for the problems we solve. 

Let d be the straight line distance the vehicle can travel between any two 
consecutive avoidance times. The cord of the smallest buffer region that is 
tangent to the obstacle it surrounds is denoted the critical cord. The critical 
cord length is given by d c = 2((R™f f ) 2 - (R^ t ) 2 ) 1/2 = ^^Va 2 - 1 because 

nmin „ pmin 

n buff ~ u£l obsf 

If d < d c , the vehicle is guaranteed to avoid the obstacle between avoidance 
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times. If d > d c , the vehicle can collide with the obstacle between avoidance 
times. The critical time interval At c is given by 



(14) 



^max 



max 



where w max is the maximum velocity of the vehicle. 

B Appendix: Minimum time MILP formulation 

Here we consider a minimum time trajectory generation problem. We are given 
a vehicle governed by the discrete time system (0 and subject to the con- 
straints J7J. The objective is to find the sequence of control inputs {uf/c]}^^ 1 
that transfers the system from the initial state x(0) = x s to the final state 
x(i/) = Xf in minimum time. 

Applying the techniques of [211 122j . we introduce a uniform time discretiza- 
tion with constant sampling time T. The solution of the resulting MILP gives 
a feasible time that is within T of the optimal time. 

Discretize time into Nt times given by <r [k] — kT, where k is an element of 
the set {1, . . . , Nt}- The discretization must be chosen so that ^[Nt] = NtT 
is larger than the optimal time. 

Next, introduce auxiliary binary variable S[k] £ {0,1} and the inequality 
constraints, 



for each k in the set {1, . . . , Nt}- Here, the state x(fy[fe]) is written in terms of 
the control inputs using equation and H is a large positive constant taken 
to be greater than the largest dimension of the operating environment. 

If S[k] — 0, every constraint in equation Ijl5|l is trivially satisfied because, 
for example, x(tT[k]) — Xf is always less than H. Otherwise, 6[k] = 1 and the 
constraints in equation l|15JI enforce the condition x(ir[fe] = x/). To require 
that the final condition be satisfied at only one discrete time tx [k] the following 
constraint is introduced, 



x(t.T[k 
x(tT[k 
y(t T [k} 
y(t T [k] 
x{pT[k 
x(tr[k 
y(t T [k] 

y(tr[k] 



x f <H(l-6[k}) 
x f >-H(l-S[k]) 
y f <H(l-6[k]) 
V S > -H(l - 6{k]) 
if < HQ. - 6[k]) 
x f >-H(l-5[k}) 
y f <H(l-6[k]) 
ijf > -HQ. - S[k]), 



(15) 




(16) 



i=l 
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Finally, we introduce the cost function to be minimized, 



J = X>M- (17) 

By minimizing this cost the final state xj is reached at the earliest discrete 
time, fr[fc], possible. The output after solving the resulting MILP is a single 
k so i such that <5[fc so i] = 1. The optimal time is therefore within the interval 
(trlksoi - l],t T [k so i}}. 
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